% Second order optimal control problem solved with a gradient method.
% The final state constraint is added to the cost function as a penalty. 
clear all;
close all;
tf = 1; % final time
dt = 0.001;
c = 200; % cost on the terminal constraint
si = [0,0]'; % initial constraint
sf=0;
%%% Optimization options
maxIter = 200; % maximum number of iterations
alpha = 0.5; % Step size in the gradient update step
tols = 1e-3;
%%% Start guess
tu = 0:dt:tf;
u = linspace(-1, 1, length(tu));
%%% Solve the problem
iter = 1; dua = 1;
while dua > tols
  %%% Simulate system forward
  [ts,s] = ode23(@ZermeloSys,[0 tf],si,[],tu,u);
  %%% Simulate adjoint system backwards
  Laf = ZermeloFinalLambda(s, sf, c);
  [tLa,La] = ode23(@ZermeloAdj,[tf 0],Laf,[],ts,s,tu,u);
  %%% Compute gradient and update the control signal
  Hu = ZermeloGrad(tLa, La, tu, u, ts, s);
  du = alpha*Hu;
  u = u - du; % update the control signal
  %%% Abort the optimization?
  if iter > maxIter, break; end
  iter = iter + 1;
  dua = norm(du)/sqrt(length(du)); 
end
%%% Show the result
x = s(:,1)';
v = s(:,2)';
fval = s(end,1);
fprintf('Final value of the objective function: %0.6f \n', fval)
figure
subplot(2,1,1), plot( ts, x, 'b+-', ts, v, 'g+-'), xlabel('t')
title('Second Order System state variables');
subplot(2,1,2), plot( tu, u, 'b-'), xlabel('t'), ylabel('u')
title('Second Order System control');
